#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "hi_pwm.h"
#include "hi_timer.h"
#include "iot_pwm.h"
#include <time.h>
#include <stdio.h>
#include "mqtt_test.h"
//左右两轮电机各由一个L9110S驱动
//GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车。
#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

//查阅机器人板原理图可知
//左边的红外传感器通过GPIO11与3861芯片连接
//右边的红外传感器通过GPIO12与3861芯片连接
#define GPIO11 11
#define GPIO12 12

#define GPIO_FUNC 0
#define car_speed_left 0
#define car_speed_right 0

extern unsigned char g_car_status;   
IotGpioValue io_status_left;
IotGpioValue io_status_right;

// 终点线相关模块
static time_t g_last_destination_time = 0;  // 上次检测到终点线的时间
static const int g_destination_timeout = 2;  // 终点线超时时间（单位：秒）
static int g_destination_counter = 0;  // 终点线检测计数
#define DESTINATION_COUNTER_MAX 100


extern float forward_speed;
extern float GetDistance();
// 新增：避障相关变量
#define DISTANCE_BETWEEN_CAR_AND_OBSTACLE 20.0  // 20cm障碍物检测距离忍受
static int g_obstacle_detected = 0;  // 障碍物检测标志
// 速度相关变量
#define MAX_SPEED_CHANGE 100
float llast_distance = 0.0;
float last_distance = 0.0;
float distance = 0.0;

// 获取小车方向
typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
    DIR_BIG_LEFT = 6,
    DIR_BIG_RIGHT = 7,
}CarDir;
extern CarDir car_dir_index;
extern void car_run(CarDir);
CarDir car_dir_index_last;

// 新增：转向执行状态变量
// 用于转向持续时间的时间戳
static int turn_start_timestamp = 0;
static int g_turning_flag = 0;  // 转向执行标志
static int g_turn_counter = 0;
int turn_duration_ms=250;



// 在文件顶部变量区添加新变量
static int first_black_detected = 0;     // 首次检测到双黑线标志
static int black_line_start_time = 0;    // 双黑线检测开始时间
#define BLACK_LINE_STRAIGHT_TIME 200     // 双黑线直行时间(ms)
static int black_line_turning_flag = 0;  // 双黑线直行状态标志
// 新增变量
#define BACKWARD_DURATION 500            // 后退持续时间(ms)
#define TURN_AFTER_BACKWARD_DURATION 70 // 后退后转向持续时间(ms)
static int black_line_backward_start_time = 0; // 后退开始时间
static int black_line_turn_after_backward_start_time = 0; // 后退后转向开始时间
static int black_line_backward_flag = 0; // 后退标志
static int black_line_turn_after_backward_flag = 0; // 后退后转向标志
static int black_line_detection_interval = 4000; // 双黑线再次检测间隔(ms)
static int last_black_line_detection_time = 0; // 上次检测到双黑线的时间

//获取红外传感器的值，调整电机的状态
void timer1_callback(unsigned int arg)
{
    IotGpioValue io_status;
    // turn_duration_ms=get_mqtt_mode();
    // 如果检测到障碍物，直接返回，不循迹(最高优先级)
    if (g_obstacle_detected) {
        return;
    }
    IoTGpioGetInputVal(GPIO11,&io_status_left); //获取GPIO11引脚的输入电平值
    IoTGpioGetInputVal(GPIO12,&io_status_right);//获取GPIO12引脚的输入电平值

    // 处理后退逻辑
    if (black_line_backward_flag) {
        if (hi_get_milli_seconds() - black_line_backward_start_time >= BACKWARD_DURATION) {
            black_line_backward_flag = 0;
            black_line_turn_after_backward_flag = 1;
            black_line_turn_after_backward_start_time = hi_get_milli_seconds();
            car_dir_index = DIR_BIG_LEFT; // 可以根据实际情况调整转向方向
        }
        return;
    }

    // 处理后退后转向逻辑
    if (black_line_turn_after_backward_flag) {
        if (hi_get_milli_seconds() - black_line_turn_after_backward_start_time >= TURN_AFTER_BACKWARD_DURATION) {
            black_line_turn_after_backward_flag = 0;
            car_dir_index = DIR_FORWARD;
        }
        return;
    }

    // 如果正在执行双黑线直行，检查是否应该结束
    if (black_line_turning_flag) {
        if (hi_get_milli_seconds() - black_line_start_time >= BLACK_LINE_STRAIGHT_TIME) {
            black_line_turning_flag = 0;
            // 直行结束后，如果还是双黑线，说明是粗停止线
            if(io_status_right == IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0) {
                printf("🛑 Thick stop line detected! Stopping car.\n");
                g_car_status = 0;
                forward_speed = 0;
                car_dir_index = DIR_STOP;
            } else {
                printf("✅ Small crossroad passed! Resume normal tracing.\n");
                first_black_detected = 0;  // 重置标志
            }
        }
        return; // 双黑线直行期间不检测其他循迹逻辑
    }

    // 如果两个红外传感器都检测到黑色
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        int current_time = hi_get_milli_seconds();
        // 短时间内再次检测到双黑线，按十字路口处理
        if (current_time - last_black_line_detection_time < black_line_detection_interval) {
            // 首次检测到双黑线
            if (!first_black_detected) {
                first_black_detected = 1;
                black_line_start_time = current_time;
                black_line_turning_flag = 1;
                g_turning_flag=0;
                car_dir_index = DIR_FORWARD;  // 强制直行
                printf("🚦 First black line detected! Go straight for %d ms\n", BLACK_LINE_STRAIGHT_TIME);
                return;  // 直行期间不检测其他逻辑
            }
        } else {
            last_black_line_detection_time = current_time;
            black_line_backward_flag = 1;
            black_line_backward_start_time = current_time;
            car_dir_index = DIR_BACKWARD;
            printf("🚦 Black line detected! Backward for %d ms\n", BACKWARD_DURATION);
            return;
        }
    } 
    // 如果检测到白线，重置首次检测标志
    else {
        if (!black_line_turning_flag) {
            first_black_detected = 0;
        }
    }
    // 如果正在执行转向，检查是否应该结束转向（第二优先级）
    if (g_turning_flag) {
        if (hi_get_milli_seconds() - turn_start_timestamp >= turn_duration_ms) {
            g_turning_flag = 0;
            //turn_start_timestamp = hi_get_milli_seconds(); // 重置时间戳
            car_dir_index = DIR_FORWARD; // 转向结束，恢复直行
        }
        return; // 转向期间不检测其他循迹逻辑
    }
    
    //value1白线 value0黑线
    if(car_dir_index == DIR_BIG_LEFT){
        if(io_status_right == IOT_GPIO_VALUE1){
            return;
        }
        else{
            // 执行右转期间不检测
            car_dir_index = DIR_BIG_RIGHT;
            g_turning_flag = 1;
            turn_start_timestamp = hi_get_milli_seconds(); // 记录新转向的开始时间
            return;
        }
    }
    if(car_dir_index == DIR_BIG_RIGHT){
        if(io_status_left == IOT_GPIO_VALUE1){
            return;
        }
        else{
            // 执行左转期间不检测
            car_dir_index = DIR_BIG_LEFT;
            g_turning_flag = 1;
            turn_start_timestamp = hi_get_milli_seconds(); // 记录新转向的开始时间
            return;
        }
    }
   
    // 如果没有偏离，直行
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0){
        car_dir_index = DIR_FORWARD;
    }
    
    //小车往左偏，则需要向右修正方向(右转)
    //如果GPIO12输入低电平（右边的红外传感器检测到黑色）并且GPIO11输入高电平（左边的红外传感器未检测到黑色）
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0)
    {   
        car_dir_index = DIR_BIG_RIGHT;
        
    } 

    //小车往右偏，则需要向左修正方向（左转）
    //如果GPIO12输入高电平（右边的红外传感器未检测到黑色）并且GPIO11输入低电平（左边的红外传感器检测到黑色）
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        car_dir_index = DIR_BIG_LEFT;
    }
}

// 检测障碍物距离和计算实时速度
void timer2_callback(void){
    // 获取前方距离
    // printf("distance:%f last_distance:%f\n",distance,last_distance);
    distance = GetDistance(); // 先获取距离
    hi_udelay(10000);
    if(car_dir_index == DIR_FORWARD){
        // printf("distance:%f last_distance:%f llast_distance:%f\n",distance,last_distance,llast_distance);
        float now_speed = (llast_distance - last_distance) * 1;
        // 添加速度范围检查
        if (now_speed > 99) {
            now_speed = 99;
        } else if (now_speed < -99) {
            now_speed = -99;
        }else{}
        // printf("now_speed:%f forward_speed:%f\n",now_speed,forward_speed);
        forward_speed = now_speed;
        // printf("now_speed:%f forward_speed:%f\n",now_speed,forward_speed);
    }
    if (car_dir_index == DIR_STOP) {
        forward_speed = 0.0;
    }
    llast_distance = last_distance;
    last_distance = distance; // 更新 last_distance
    // 检查是否有障碍物
    if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
        if (!g_obstacle_detected) {
            printf("🚨 Obstacle detected! Distance: %.2f cm\n", distance);
            g_obstacle_detected = 1;
            // 立即停止小车
            car_dir_index_last = car_dir_index;
            car_dir_index = DIR_STOP;
            forward_speed = 0;
        }
    } else {
        if (g_obstacle_detected) {
            printf("✅ Obstacle cleared! Distance: %.2f cm, resuming trace\n", distance);
            g_obstacle_detected = 0;
            car_dir_index = car_dir_index_last;
        }
    }
}

void trace_module(void)
{  
    regress_middle();
    g_destination_counter=0;
    unsigned int timer_id1;
    hi_timer_create(&timer_id1);
    unsigned int timer_id2;
    hi_timer_create(&timer_id2);
    
     
    //启动系统周期定时器用来按照预定的时间间隔1ms触发timer1_callback任务的执行
    hi_timer_start(timer_id1, HI_TIMER_TYPE_PERIOD, 1, timer1_callback, 0);
    hi_timer_start(timer_id2, HI_TIMER_TYPE_PERIOD, 200, timer2_callback, 0);

     
    car_dir_index = DIR_FORWARD;
    
    while (1) { 
        car_run(car_dir_index);

        hi_udelay(20);

        if (g_car_status != CAR_TRACE_STATUS) {
            break;
        }
    }
    hi_timer_delete(timer_id1);
    hi_timer_delete(timer_id2);
   
}


 